#include "PID.h"

PID_param pid;

/**
 * @brief PID参数初始化
 * @param  无
 * @return 无
 */
void PID_Init()
{
    pid.axis_kp=0;
    pid.axis_ki=0;
    pid.axis_kd=0;
}

/**
 * @brief 关节PID算法控制
 * @param *joint_angles 逆解算需要变化的舵机角度值
 * @param pid_angles_bias[] 舵机上一次的角度误差
 * @param steer_out[] 舵机经过pid调节后输出的角度偏移
 * @return 无
 */
void Axis_Pid_Control(JointAngles *joint_angles,double pid_angles_bias[],double steer_out[])
{
    double angle_bias;
    static double last_angle_bias;
    static double angle_integral;
    for (uint8_t i = 0; i < Axis_num; i++)
    {
        switch (i)
        {
        case 0:
            angle_bias = joint_angles->theta1;
            last_angle_bias = pid_angles_bias[i];
            break;
        case 1:
            angle_bias = joint_angles->theta2;
            last_angle_bias = pid_angles_bias[i];
            break;    
        case 2:
            angle_bias = joint_angles->theta3;
            last_angle_bias = pid_angles_bias[i];
            break;
        case 3:
            angle_bias = joint_angles->theta4;
            last_angle_bias = pid_angles_bias[i];
            break;
        case 4:
            angle_bias = joint_angles->theta5;
            last_angle_bias = pid_angles_bias[i];
            break;

        }

        angle_integral += angle_bias;
        if(angle_integral>=10)
        {
            angle_integral=10;
        }

        steer_out[i] = pid.axis_kp * angle_bias + pid.axis_ki * angle_integral + pid.axis_kd * (angle_bias - last_angle_bias);
        pid_angles_bias[i] = angle_bias;

    }
    
    
}
